﻿#include "census.h"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <intrin.h>
using cv::Mat;
void Census::operator()(const cv::Mat& src, cv::Mat& dst)
{
	if (src.type() == CV_8UC1)
	{
		census_8u(src, dst);
	}
	else if (src.type() == CV_16UC1)
	{
		census_16u(src, dst);
	}
	else if (src.type() == CV_32FC1)
	{
		census_32f(src, dst);
	}
}

void Census::census_8u(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_8U
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	//生成一个16 byte aligned mat，为了采用SSE计算
	//相当于是增加了15-32像素，为了保证边缘也能计算，而且也align
	int aligned_cols = cols + 32 - (cols & 16);

	cv::Mat src_aligned = cv::Mat::zeros(rows, aligned_cols, CV_8UC1);
	src.copyTo(src_aligned(cv::Range(0, rows), cv::Range(0, cols)));

	//创建dst 的buffer， 即census变换影像，一共有对于9X7窗口，一共有62bit，采用8字节来存储
	cv::Mat dst_aligned = cv::Mat::zeros(rows, aligned_cols, CV_8UC(8));

	__m128i bitconst = _mm_set1_epi8(1);
	__m128i bitmask;
	//计算策略是这样的，先计算--------,每一横杠代表16像素的8bit的值
	//然后再interleave过来，使每一像素的值，都连续起来
	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		uint8_t* src_row = (uint8_t*)src_aligned.ptr(r);
		cv::Vec<uint8_t, 8>* dst_row = (cv::Vec<uint8_t, 8>*)dst_aligned.ptr(r);
		//对于8bit的影像，一次操作16像素
		for (int c = 0; c < cols - param_.window_x; c += 16)
		{
			__m128i src_packed_pixels = _mm_lddqu_si128((__m128i*)src_aligned.ptr<uint8_t>(r, c + col_edge));
			__m128i* dst_stride_pixels = (__m128i*)(dst_row+c);
			
			bitmask = bitconst;
			int bitcount = 0;
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					//中心像素就不比较了
					if (c_census == 0 && r_census == 0)
					{
						continue;
					}
					uint8_t* src_data = src_aligned.ptr<uint8_t>(r + r_census, c + col_edge + c_census);
					//由于这个像素的位置可能不是16align，所以需要load进来
					__m128i comp_packed_pixels = _mm_lddqu_si128((__m128i*)src_data);

					//计算16个像素的值和src_packed_pixels的大小，并且存到
					*dst_stride_pixels = _mm_or_si128(*dst_stride_pixels, _mm_and_si128(_mm_cmpeq_epi8(
						_mm_max_epu8(comp_packed_pixels, src_packed_pixels), comp_packed_pixels), bitmask));
					//bitmask 进一位
					bitmask = _mm_add_epi8(bitmask, bitmask);
					++bitcount;
					if (bitcount == 8)
					{
						//一个BYTE存完了，现在每个像素都进一个BYTE
						++dst_stride_pixels;
						bitmask = bitconst;
						bitcount = 0;
					}
				}
			}
			//这16个像素的值都算完了，但是这个内存是分开的，需要把每个像素的bit都调连续了
			store_8u64b((__m128i*)dst_aligned.ptr<uint64_t>(r, c), (__m128i*)dst_aligned.ptr<uint64_t>(r, c));
		}
	}
	dst_aligned(cv::Range(row_edge, rows - row_edge), cv::Range(0, cols - param_.window_x + 1)).clone().copyTo(
		dst(cv::Range(row_edge, rows - row_edge), cv::Range(col_edge, cols - col_edge)));
}

void Census::census_16u(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_16U
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	//生成一个16 byte aligned mat，为了采用SSE计算
	//相当于是增加了15-32像素，为了保证边缘也能计算，而且也align
	int aligned_cols = cols + 32 - (cols & 16);

	cv::Mat src_aligned = cv::Mat::zeros(rows, aligned_cols, CV_16UC1);
	src.copyTo(src_aligned(cv::Range(0, rows), cv::Range(0, cols)));

	//创建dst 的buffer， 即census变换影像，一共有对于9X7窗口，一共有62bit，采用8字节来存储
	cv::Mat dst_aligned = cv::Mat::zeros(rows, aligned_cols, CV_8UC(8));

	__m128i bitconst = _mm_set1_epi16(1);
	__m128i bitmask;
	//计算策略是这样的，先计算--------,每一横杠代表8像素的8bit的值
	//然后再interleave过来，使每一像素的值，都连续起来
	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		uint16_t* src_row = (uint16_t*)src_aligned.ptr(r);
		cv::Vec<uint8_t, 8>* dst_row = (cv::Vec<uint8_t, 8>*)dst_aligned.ptr(r);
		//对于16bit的影像，一次操作8像素
		for (int c = 0; c < cols - param_.window_x; c += 8)
		{
			//src_packed_pixels pack了8个像素
			__m128i src_packed_pixels = _mm_lddqu_si128((__m128i*)src_aligned.ptr<uint16_t>(r, c + col_edge));
			//+c实质上内存地址增加为8 * c, 因为C整除8所以仍然是16BYTE aligned
			__m128i* dst_stride_pixels = (__m128i*)(dst_row + c); 

			bitmask = bitconst;
			int bitcount = 0;
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					//中心像素就不比较了
					if (r_census == 0 && c_census == 0)
					{
						continue;
					}
					uint16_t* src_data = src_aligned.ptr<uint16_t>(r + r_census, c + col_edge + c_census);
					//由于这个像素的位置可能不是16align，所以需要load进来
					__m128i comp_packed_pixels = _mm_lddqu_si128((__m128i*)src_data);

					//计算8个像素的值和src_packed_pixels的大小，并且存到
					*dst_stride_pixels = _mm_or_si128(*dst_stride_pixels, _mm_and_si128(_mm_cmpeq_epi16(
						_mm_max_epu16(comp_packed_pixels, src_packed_pixels), comp_packed_pixels), bitmask));
					//bitmask 进一位
					bitmask = _mm_add_epi16(bitmask, bitmask);
					++bitcount;
					if (bitcount == 16)
					{
						//这个__m128i里面存了8个像素，每个像素有16bit的比较值
						++dst_stride_pixels;
						bitmask = bitconst;
						bitcount = 0;
					}
				}
			}
			//这8个像素的值都算完了，但是这个内存是分开的，需要把每个像素的bit都调连续了
			//8个像素，存在邻近的4个__m128i中
			store_16u64b((__m128i*)dst_aligned.ptr<uint64_t>(r, c), (__m128i*)dst_aligned.ptr<uint64_t>(r, c));
		}
	}
	dst_aligned(cv::Range(row_edge, rows - row_edge), cv::Range(0, cols - param_.window_x + 1)).clone().copyTo(
		dst(cv::Range(row_edge, rows - row_edge), cv::Range(col_edge, cols - col_edge)));
}

void Census::census_32f(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_32F
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	//生成一个16 byte aligned mat，为了采用SSE计算
	//相当于是增加了15-32像素，为了保证边缘也能计算，而且也align
	int aligned_cols = cols + 32 - (cols & 16);

	cv::Mat src_aligned = cv::Mat::zeros(rows, aligned_cols, CV_32FC1);
	src.copyTo(src_aligned(cv::Range(0, rows), cv::Range(0, cols)));

	//创建dst 的buffer， 即census变换影像，一共有对于9X7窗口，一共有62bit，采用8字节来存储
	cv::Mat dst_aligned = cv::Mat::zeros(rows, aligned_cols, CV_8UC(8));


	__m128i bitconst = _mm_set1_epi32(1);
	__m128i bitmask;
	//计算策略是这样的，先计算----,每一横杠代表4像素的32bit的值
	//然后再interleave过来，使每一像素的值，都连续起来
	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		float* src_row = (float*)src_aligned.ptr(r);
		cv::Vec<uint8_t, 8>* dst_row = (cv::Vec<uint8_t, 8>*)dst_aligned.ptr(r);
		//对于32bit的影像，一次操作4像素
		for (int c = 0; c < cols - param_.window_x; c += 4)
		{
			//src_packed_pixels pack了4个像素
			__m128 src_packed_pixels = _mm_loadu_ps((float*)src_aligned.ptr<float>(r, c + col_edge));
			//+c实质上内存地址增加为64*c, 所以仍然是16BYTE aligned
			__m128i* dst_stride_pixels = (__m128i*)(dst_row + c);

			bitmask = bitconst;
			int bitcount = 0;
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					//中心像素就不比较了
					if (r_census == 0 && c_census == 0)
					{
						continue;
					}
					float* src_data = src_aligned.ptr<float>(r + r_census, c + col_edge + c_census);
					//由于这个像素的位置可能不是16align，所以需要load进来
					__m128 comp_packed_pixels = _mm_loadu_ps(src_data);

					//计算4个像素的值和src_packed_pixels的大小，如果大于等于src_packed_pixels，则记一个1
					__m128i comp_results = _mm_castps_si128(_mm_cmpge_ps(comp_packed_pixels, src_packed_pixels));
					
					//通过一个与操作，和一个或操作，如果大与等于，那么在对应的位置会变为1
					*dst_stride_pixels = _mm_or_si128(*dst_stride_pixels, _mm_and_si128(comp_results, bitmask));
					
					//bitmask 进一位
					bitmask = _mm_add_epi32(bitmask, bitmask);
					++bitcount;
					if (bitcount == 32)
					{
						//这个__m128i里面存了4个像素，每个像素有32bit的比较值
						++dst_stride_pixels;
						bitmask = bitconst;
						bitcount = 0;
					}
				}
			}
			//这8个像素的值都算完了，但是这个内存是分开的，需要把每个像素的bit都调连续了
			//8个像素，存在邻近的4个__m128i中
			store_32u64b((__m128i*)dst_aligned.ptr<uint64_t>(r, c), (__m128i*)dst_aligned.ptr<uint64_t>(r, c));
		}
	}
	dst_aligned(cv::Range(row_edge, rows - row_edge), cv::Range(0, cols - param_.window_x + 1)).clone().copyTo(
		dst(cv::Range(row_edge, rows - row_edge), cv::Range(col_edge, cols - col_edge)));
}

void Census::store_8u64b(__m128i* src, __m128i* dst)
{
	__m128i lo01 = _mm_unpacklo_epi8(*src, *(src + 1)); //0011...77
	__m128i hi01 = _mm_unpackhi_epi8(*src, *(src + 1)); //8899..1515

	__m128i lo23 = _mm_unpacklo_epi8(*(src + 2), *(src + 3)); //0011...77
	__m128i hi23 = _mm_unpackhi_epi8(*(src + 2), *(src + 3));//8899..1515

	__m128i lo45 = _mm_unpacklo_epi8(*(src + 4), *(src + 5));//0011...77
	__m128i hi45 = _mm_unpackhi_epi8(*(src + 4), *(src + 5));//8899..1515

	__m128i lo67 = _mm_unpacklo_epi8(*(src + 6), *(src + 7));//0011...77
	__m128i hi67 = _mm_unpackhi_epi8(*(src + 6), *(src + 7));//8899..1515

	__m128i lo0123 = _mm_unpacklo_epi16(lo01, lo23);//0000111122223333
	__m128i lo4567 = _mm_unpackhi_epi16(lo01, lo23);//4444555566667777

	__m128i hi0123 = _mm_unpacklo_epi16(lo45, lo67);//0000111122223333
	__m128i hi4567 = _mm_unpackhi_epi16(lo45, lo67);//4444555566667777

	__m128i pixel01 = _mm_unpacklo_epi32(lo0123, hi0123);//0000000011111111
	__m128i pixel23 = _mm_unpackhi_epi32(lo0123, hi0123);//2222222233333333
	__m128i pixel45 = _mm_unpacklo_epi32(lo4567, hi4567);//4444444455555555
	__m128i pixel67 = _mm_unpackhi_epi32(lo4567, hi4567);//6666666677777777

	__m128i lo08_11 = _mm_unpacklo_epi16(hi01, hi23);//88889999...11x4
	__m128i lo12_15 = _mm_unpackhi_epi16(hi01, hi23);//12x4...15x4	
	__m128i hi08_11 = _mm_unpacklo_epi16(hi45, hi67);//88889999...11x4
	__m128i hi12_15 = _mm_unpackhi_epi16(hi45, hi67);//12x4...15x4

	__m128i pixel89 = _mm_unpacklo_epi32(lo08_11, hi08_11);//8888888899999999
	__m128i pixel1011 = _mm_unpackhi_epi32(lo08_11, hi08_11);//10x8...11x8
	__m128i pixel1213 = _mm_unpacklo_epi32(lo12_15, hi12_15);//12x8...13x8
	__m128i pixel1415 = _mm_unpackhi_epi32(lo12_15, hi12_15);//14x8...15x8

	_mm_store_si128(dst, pixel01);
	_mm_store_si128(dst+1, pixel23);
	_mm_store_si128(dst+2, pixel45);
	_mm_store_si128(dst+3, pixel67);
	_mm_store_si128(dst+4, pixel89);
	_mm_store_si128(dst+5, pixel1011);
	_mm_store_si128(dst+6, pixel1213);
	_mm_store_si128(dst+7, pixel1415);
}

void Census::store_16u64b(__m128i* src, __m128i* dst)
{
	//src每一个128存的是0011223344556677,一共存储8个像素的比较值
	__m128i lo0123 = _mm_unpacklo_epi16(*src, *(src+1));//0000111122223333
	__m128i lo4567 = _mm_unpackhi_epi16(*src, *(src+1));//4444555566667777

	__m128i hi0123 = _mm_unpacklo_epi16(*(src+2), *(src+3));//0000111122223333
	__m128i hi4567 = _mm_unpackhi_epi16(*(src+2), *(src+3));//4444555566667777

	__m128i pixel01 = _mm_unpacklo_epi32(lo0123, hi0123);//0000000011111111
	__m128i pixel23 = _mm_unpackhi_epi32(lo0123, hi0123);//2222222233333333
	__m128i pixel45 = _mm_unpacklo_epi32(lo4567, hi4567);//4444444455555555
	__m128i pixel67 = _mm_unpackhi_epi32(lo4567, hi4567);//6666666677777777

	_mm_store_si128(dst, pixel01);
	_mm_store_si128(dst + 1, pixel23);
	_mm_store_si128(dst + 2, pixel45);
	_mm_store_si128(dst + 3, pixel67);
}


void Census::store_32u64b(__m128i* src, __m128i* dst)
{
	//src每一个128存的是0000111122223333,一共存储4个像素的比较值

	__m128i pixel01 = _mm_unpacklo_epi32(*src, *(src+1));//0000000011111111
	__m128i pixel23 = _mm_unpackhi_epi32(*src, *(src + 1));//2222222233333333
	
	_mm_store_si128(dst, pixel01);
	_mm_store_si128(dst + 1, pixel23);
	}

void Census::serial(const cv::Mat& src, cv::Mat& dst)
{
	if (src.type() == CV_8UC1)
	{
		census_8u_serial(src, dst);
	}
	else if (src.type() == CV_16UC1)
	{
		census_16u_serial(src, dst);
	}
	else if (src.type() == CV_32FC1)
	{
		census_32f_serial(src, dst);
	}
	
}

void Census::census_8u_serial(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_8U
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	uchar mask = 0xff;
	
	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		for (int c = col_edge; c < cols - col_edge; ++c)
		{
			int byte_pos = 0;
			int bitcount = 0;
			const uchar* src_data = src.ptr<uchar>(r,c);
			cv::Vec<uchar, 8>* dst_data = dst.ptr<cv::Vec<uchar, 8>>(r,c);
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					if (r_census == 0 && c_census == 0)
					{
						continue;
					}
					uchar comp = src.at<uchar>(r + r_census, c + c_census);
					(*dst_data)[byte_pos] = (*dst_data)[byte_pos] | ((comp >= *src_data) << bitcount);
					++bitcount;
					if (bitcount == 8)
					{
						byte_pos++;
						bitcount = 0;
					}				
				}
			}			
		}
	}
}

void Census::census_16u_serial(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_16U
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	uchar mask = 0xff;

	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		for (int c = col_edge; c < cols - col_edge; ++c)
		{
			int byte_pos = 0;
			int bitcount = 0;
			const uint16_t* src_data = src.ptr<uint16_t>(r, c);
			cv::Vec<uint8_t, 8>* dst_data = dst.ptr<cv::Vec<uint8_t, 8>>(r, c);
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					if (r_census == 0 && c_census == 0)
					{
						continue;
					}
					uint16_t comp = src.at<uint16_t>(r + r_census, c + c_census);
					(*dst_data)[byte_pos] = (*dst_data)[byte_pos] | ((comp >= *src_data) << bitcount);
					++bitcount;
					if (bitcount == 16)
					{
						byte_pos++;
						bitcount = 0;
					}
				}
			}
		}
	}
}

void Census::census_32f_serial(const cv::Mat& src, cv::Mat& dst)
{
	//这里src是CV_32F
	int rows = src.rows;
	int cols = src.cols;

	dst = cv::Mat::zeros(rows, cols, CV_8UC(8));

	//在影像四周的范围内，都不计算census值
	int row_edge = param_.window_y * 0.5;
	int col_edge = param_.window_x * 0.5;

	uchar mask = 0xff;

	for (int r = row_edge; r < rows - row_edge; ++r)
	{
		for (int c = col_edge; c < cols - col_edge; ++c)
		{
			int byte_pos = 0;
			int bitcount = 0;
			const float* src_data = src.ptr<float>(r, c);
			cv::Vec<uint8_t, 8>* dst_data = dst.ptr<cv::Vec<uint8_t, 8>>(r, c);
			for (int r_census = -row_edge; r_census <= row_edge; ++r_census)
			{
				for (int c_census = -col_edge; c_census <= col_edge; ++c_census)
				{
					if (r_census == 0 && c_census == 0)
					{
						continue;
					}
					float comp = src.at<float>(r + r_census, c + c_census);
					(*dst_data)[byte_pos] = (*dst_data)[byte_pos] | ((comp >= *src_data) << bitcount);
					++bitcount;
					if (bitcount == 8)
					{
						byte_pos++;
						bitcount = 0;
					}
				}
			}
		}
	}
}
